//
// Created by bill3000 on 3/4/21.
//

#include <unistd.h>

#include "fruit.hpp"
#include "../log/logger.hpp"

int FruitFSM::learning_start_size_auto_learning_thread(int cn) {
    int ret;
    pthread_t id;
    pthread_attr_t thread_attr;

    ret = pthread_attr_init(&thread_attr);
    std::string ss = "Failed to create auto size learning thread !";
    if (ret != 0) {
        LOG(ERROR, ss);
        return -1;
    }
    ret = pthread_attr_setdetachstate(&thread_attr, PTHREAD_CREATE_DETACHED);
    if (ret != 0) {
        LOG(ERROR, ss);
        return -1;
    }
    ret = pthread_create(&id, &thread_attr, learning_size_auto_learning_thread, (void *)&cn);
    if(ret!=0) {
        LOG(ERROR, ss);
        return -1;
    }
    pthread_attr_destroy(&thread_attr);

    return 0;
}

void * FruitFSM::learning_size_auto_learning_thread(void *data) {
    int catalog_num = *(int *)data;
    if ((catalog_num < 1) || (catalog_num > 3)) catalog_num = 3;

    int begin = 0, end = 0;
    cv::Mat m1, m2, m3;

    std::map<std::string, int> cam1_name_area_map, cam2_name_area_map, cam3_name_area_map;

    action_start_conveyor(nullptr);

    while(learning) {
        usleep(50000); //delay 50ms

        *cam1 >> m1; *cam2 >> m2; *cam3 >> m3;
        if (m1.empty() || m2.empty() || m3.empty()) {
            LOG(ERROR, "Capture image error !");
            learning = false;
            break;
        }
        int area_threshold = m1.rows * m1.cols / 9;

        int raw_area = algorithm_get_image_area(m1);
        int area = 0;
        if (raw_area > area_threshold)
            area = raw_area;

        if (area == 0) {
            if (begin == 0) {  //a new round of start
                begin = 1;
                continue;
            }
            if (end == 1) {
                //Here, we got a perfect series of a fruit images
                //and we must get the one in the middle of the series
                learning_sal_get_perfect_image_from_series(cam1_name_area_map,
                                                           cam2_name_area_map,
                                                           cam3_name_area_map);

                begin = 0;  //start a new round
                end = 0;
            }
            continue;
        }
        if (begin == 0) continue;

        //We get a fruit image and put it in the "/intelli/tracking/size/" directory
        end = 1;
        std::string fn = util_get_now_time_milli_string();
        fn.insert(0, "/intelli/tracking/size/");
        std::string fn1 = fn, fn2 = fn, fn3 = fn;
        fn1.append("-01.jpg"); fn2.append("-02.jpg"); fn3.append("-03.jpg");
        cv::imwrite(fn1, m1); cv::imwrite(fn2, m2); cv::imwrite(fn3, m3);
    }
    util_delete_dir_content("/intelli/tracking/size/");

    //we have got all the needed fruit imges, we learn know.
    learned_size_model[0] = algorithm_sal_kmeans_learning(cam1_name_area_map, catalog_num, "1/");
    learned_size_model[0].ls.camid = 1;
    learned_size_model[1] = algorithm_sal_kmeans_learning(cam2_name_area_map, catalog_num, "2/");
    learned_size_model[1].ls.camid = 2;
    learned_size_model[2] = algorithm_sal_kmeans_learning(cam3_name_area_map, catalog_num, "3/");
    learned_size_model[2].ls.camid = 3;

    //Notify FSM we finished learning
    Message msg;
    msg.mtype = 300;
    msg.data.ctype = EVENT_LEARNING_SAL_COMPLETED;
    mq->send_message(M02, &msg);

    util_delete_dir_content("/intelli/working/");

    action_stop_conveyor(nullptr);

    pthread_exit(nullptr);
}

int FruitFSM::learning_start_size_semi_learning_thread(int size) {
    int ret;
    pthread_t id;
    pthread_attr_t thread_attr;

    ret = pthread_attr_init(&thread_attr);
    std::string ss = "Failed to create size semi learning thread !";
    if (ret != 0) {
        LOG(ERROR, ss);
        return -1;
    }
    ret = pthread_attr_setdetachstate(&thread_attr, PTHREAD_CREATE_DETACHED);
    if (ret != 0) {
        LOG(ERROR, ss);
        return -1;
    }
    ret = pthread_create(&id, &thread_attr, learning_size_semi_learning_thread, (void *)&size);
    if(ret!=0) {
        LOG(ERROR, ss);
        return -1;
    }
    pthread_attr_destroy(&thread_attr);

    return 0;
}

void * FruitFSM::learning_size_semi_learning_thread(void *data) {
    int size = *(int *)data;

    int begin = 0, end = 0;
    cv::Mat m1, m2, m3, m;
    std::map<std::string, int> cam1_name_area_map, cam2_name_area_map, cam3_name_area_map;

    action_start_conveyor(nullptr);

    while(learning) {
        usleep(50000); //delay 50ms

        *cam1 >> m1; *cam2 >> m2; *cam3 >> m3;
        if (m1.empty() || m2.empty() || m3.empty()) {
            LOG(ERROR, "Capture image error !");
            learning = false;
            break;
        }
        int area_threshold = m1.rows * m1.cols / 9;

        int raw_area = algorithm_get_image_area(m1);
        int area = 0;
        if (raw_area > area_threshold)
            area = raw_area;

        if (area == 0) {
            if (begin == 0) {  //a new round of start
                begin = 1;
                continue;
            }
            if (end == 1) {
                //Here, we got a perfect series of a fruit images
                //and we must get the one in the middle of the series
                //Note: we put the perfect image in destination dir = /intelli/working/
                learning_sal_get_perfect_image_from_series(cam1_name_area_map,
                                                           cam2_name_area_map,
                                                           cam3_name_area_map);

                begin = 0;  //start a new round
                end = 0;
            }
            continue;
        }
        if (begin == 0) continue;

        //We get a fruit image and put it in the "/intelli/tracking/size/" directory
        end = 1;
        std::string fn = util_get_now_time_milli_string();
        fn.insert(0, "/intelli/tracking/size/");
        std::string fn1 = fn, fn2 = fn, fn3 = fn;
        fn1.append("-01.jpg"); fn2.append("-02.jpg"); fn3.append("-03.jpg");
        cv::imwrite(fn1, m1); cv::imwrite(fn2, m2); cv::imwrite(fn3, m3);
    }
    util_delete_dir_content("/intelli/tracking/size/");

    //we have got all the needed fruit imges
    //in cam1_name_area_map, cam1_name_area_map, cam1_name_area_map respectively
    learning_ssl_copy_file_to_dest_and_compute_means(size, cam1_name_area_map, cam2_name_area_map, cam3_name_area_map);

    //Notify FSM we finished learning
    Message msg;
    msg.mtype = 300;
    msg.data.ctype = EVENT_LEARNING_SSL_COMPLETED;
    msg.data.param = size;
    mq->send_message(M02, &msg);

    util_delete_dir_content("/intelli/working/");
    action_stop_conveyor(nullptr);

    pthread_exit(nullptr);
}

void FruitFSM::learning_sal_get_perfect_image_from_series(std::map<std::string, int> &cam1_name_area_map,
                                                          std::map<std::string, int> &cam2_name_area_map,
                                                          std::map<std::string, int> &cam3_name_area_map) {
    std::vector<std::string> fns;
    std::string sdir = "/intelli/tracking/size/";
    util_get_dir_content(sdir, fns);

    unsigned long mid = fns.size() / 2;
    std::string fn = fns.at(mid);
    fn = fn.substr(0, fn.find_last_of('-'));
    std::string fn1 = fn, fn2 = fn, fn3 = fn;
    fn1.append("-01.jpg"); fn2.append("-02.jpg"); fn3.append("-03.jpg");

    std::string ddir = "/intelli/working/";
    if ((util_copy_file(sdir, fn1, ddir) < 0) ||
        (util_copy_file(sdir, fn2, ddir) < 0) ||
        (util_copy_file(sdir, fn3, ddir) < 0)) {
        LOG(ERROR, "Copy file error, Can not learn anymore !");
        learning = false;
        pthread_exit(nullptr);
    }
    util_delete_dir_content("/intelli/tracking/size/");

    cv::Mat m1, m2, m3;
    m1 = cv::imread(ddir + fn1); m2 = cv::imread(ddir + fn2); m3 = cv::imread(ddir + fn2);
    if (m1.empty() || m2.empty() || m3.empty()) {
        LOG(ERROR, "Read image file error, Cannot learn anymore !");
        learning = false;
        pthread_exit(nullptr);
    }
    cam1_name_area_map.insert(std::make_pair(fn1, algorithm_get_image_area(m1)));
    cam2_name_area_map.insert(std::make_pair(fn2, algorithm_get_image_area(m2)));
    cam3_name_area_map.insert(std::make_pair(fn3, algorithm_get_image_area(m3)));
}

void FruitFSM::learning_ssl_copy_file_to_dest_and_compute_means(int size,
                                                                std::map<std::string, int> &cam1_name_area_map,
                                                                std::map<std::string, int> &cam2_name_area_map,
                                                                std::map<std::string, int> &cam3_name_area_map) {
    float total = 0;
    if (size == 1) { //Large
        for(auto & iter : cam1_name_area_map) {
            util_copy_file("/intelli/working/", iter.first, "/intelli/learning/size/1/large/");
            total += iter.second;
        }
        learned_size_model[0].ls.large = (int)(total / cam1_name_area_map.size());

        total = 0;
        for(auto & iter : cam2_name_area_map) {
            util_copy_file("/intelli/working/", iter.first, "/intelli/learning/size/2/large/");
            total += iter.second;
        }
        learned_size_model[1].ls.large = (int)(total / cam2_name_area_map.size());

        total = 0;
        for(auto & iter : cam3_name_area_map) {
            util_copy_file("/intelli/working/", iter.first, "/intelli/learning/size/3/large/");
            total += iter.second;
        }
        learned_size_model[2].ls.large = (int)(total / cam3_name_area_map.size());
    }
    else if (size == 2) {  //Middle
        for(auto & iter : cam1_name_area_map) {
            util_copy_file("/intelli/working/", iter.first, "/intelli/learning/size/1/middle/");
            total += iter.second;
        }
        learned_size_model[0].ls.middle = (int)(total / cam1_name_area_map.size());

        total = 0;
        for(auto & iter : cam2_name_area_map) {
            util_copy_file("/intelli/working/", iter.first, "/intelli/learning/size/2/middle/");
            total += iter.second;
        }
        learned_size_model[1].ls.middle = (int)(total / cam2_name_area_map.size());

        total = 0;
        for(auto & iter : cam3_name_area_map) {
            util_copy_file("/intelli/working/", iter.first, "/intelli/learning/size/3/middle/");
            total += iter.second;
        }
        learned_size_model[2].ls.middle = (int)(total / cam3_name_area_map.size());
    }
    else {  //small
        for(auto & iter : cam1_name_area_map) {
            util_copy_file("/intelli/working/", iter.first, "/intelli/learning/size/1/small/");
            total += iter.second;
        }
        learned_size_model[0].ls.small = (int)(total / cam1_name_area_map.size());

        total = 0;
        for(auto & iter : cam2_name_area_map) {
            util_copy_file("/intelli/working/", iter.first, "/intelli/learning/size/2/small/");
            total += iter.second;
        }
        learned_size_model[1].ls.small = (int)(total / cam2_name_area_map.size());

        total = 0;
        for(auto & iter : cam3_name_area_map) {
            util_copy_file("/intelli/working/", iter.first, "/intelli/learning/size/3/small/");
            total += iter.second;
        }
        learned_size_model[2].ls.small = (int)(total / cam3_name_area_map.size());
    }
}
